#include <Servo.h>

/****************************/
/* servo parts              */
/****************************/
#define N_HEAD 2
#define I_HEAD_NECK1 0
#define I_HEAD_NECK2 1

#define N_ARM 4
#define I_ARM_SHOULDER 0
#define I_ARM_BICEP 1
#define I_ARM_ELBOW 2
#define I_ARM_WRIST 3

#define N_LEG 5
#define I_LEG_HIP1 0
#define I_LEG_HIP2 1
#define I_LEG_THIGH 2
#define I_LEG_KNEE 3
#define I_LEG_ANKLE 4

Servo servo_head[N_HEAD];
int ns_head[N_HEAD] = {
  26, 27};
int pos_head[N_HEAD] = {
  90, 90};

Servo servo_left_arm[N_ARM];
int ns_left_arm[N_ARM] = {
  46, 47, 48, 49};
int pos_left_arm[N_ARM] = {
  90-60, 180, 180, 90};

Servo servo_right_arm[N_ARM];
int ns_right_arm[N_ARM] = {
  33, 34, 35, 36};
int pos_right_arm[N_ARM] = {
  90+60, 0, 0, 90};

Servo servo_left_leg[N_LEG];
int ns_left_leg[N_LEG] = {
  41, 42, 43, 44, 45};
int pos_left_leg[N_LEG] = {
  90, 90, 90, 90, 90};

Servo servo_right_leg[N_LEG];
int ns_right_leg[N_LEG] = {
  28, 29, 30, 31, 32};
int pos_right_leg[N_LEG] = {
  90, 90, 90, 90, 90};

/****************************/
/* positions                */
/****************************/
// current position !
#define _POSITION_CURRENT 0

// static stands
#define _POSITION_RIGHT 0x0100
#define _POSITION_STAND 0x0101
#define _POSITION_WAITFOR 0x0102

// hello stands
#define _POSITION_HELLO1 0x0200
#define _POSITION_HELLO2 0x0201

// sporty
#define _POSITION_PUMP0 0x0300
#define _POSITION_PUMP1 0x0301
#define _POSITION_SEMIPUMP0 0x0310
#define _POSITION_SEMIPUMP1 0x0311

// exotic :)
#define _POSITION_HULK 0x0910
#define _POSITION_PLANE 0x0920

class AnimLoopStep
{
  public:
  unsigned long _iTime;  
  int _iPos1, _iPos2;
  AnimLoopStep()
  {
    _iTime = 0;
    _iPos1 = 0;
    _iPos2 = 0;
  }
  AnimLoopStep(unsigned long iTime, int iPos1, int iPos2)
  {
    _iTime = iTime;
    _iPos1 = iPos1;
    _iPos2 = iPos2;    
  }
};

class AnimLoop
{
  public:
  char name[16];
  int _iSteps;
  AnimLoopStep _steps[10];
  AnimLoop()
  {
    strcpy(name, "anim");
    _iSteps = 0;
  }
};

#define N_ANIM_LOOPS 5
AnimLoop loops[N_ANIM_LOOPS];

unsigned long g_time=0;
void setup()
{
  Serial.begin(9600);
  Serial1.begin(9600);

  init_servos();
  init_loops();
  
  Serial.println("You are in manual mode, press 'a' to switch to loops mode.");
 
  g_time = millis();

  set_animation_step(500, _POSITION_RIGHT, _POSITION_RIGHT, 0);
}

boolean g_anim = false;
int g_what_anim = 0;
void loop()
{
  // first update servo position
  update_servos();

  // set next position
  unsigned long cur_time = millis();
  unsigned long slicetime = cur_time-g_time;
  g_time = cur_time;

  boolean anim_ended = do_anim(slicetime);
  // if anim isn't finish continue :)
  if(!anim_ended) return;

  // got bluetooth command ?
  if(check_commands()) return;

  // else...
  inDemoMode();
}

void inDemoMode()
{
  int val = 0;
  if(Serial.available())
    val = Serial.read();
  if(val=='a')
  {
    g_anim = !g_anim;
    if(g_anim)
    {
      Serial.print("This is LOOPS mode !");
      Serial.print(" press 0 to 4 for select a");
      Serial.println(" kind of animation...");
    }
    else
    {
      Serial.print("This is MANUEL mode !");
      Serial.print(" press 0 or +/- to select a position step");
      Serial.println(" and * to activate");
    }
  }

  if(g_anim)
  {
    static int anim_step = 0;
    int iVal = -1;
    if(val>0)
    {
      char v[2] = {val, 0};
      iVal = atoi(v);
    }
    if(iVal>=0 && iVal<=(N_ANIM_LOOPS-1))
    {
      g_what_anim = iVal;

      Serial.print("Anim ");
      Serial.print(g_what_anim);
      Serial.print(" : '");
      Serial.print(loops[g_what_anim].name);
      Serial.println("' has been choosen !");
      anim_step=0;
      set_animation_step(loops[g_what_anim]._steps[anim_step]._iTime, loops[g_what_anim]._steps[anim_step]._iPos1, loops[g_what_anim]._steps[anim_step]._iPos2, 0);
      return;
    }

    set_animation_time(0);
    anim_step++;
    if(anim_step>(loops[g_what_anim]._iSteps-1)) anim_step = 0;
    
    Serial.print("Anim '");
    Serial.print(loops[g_what_anim].name);;
    Serial.print("' step ");
    Serial.print(anim_step+1);
    Serial.print("/");
    Serial.print(loops[g_what_anim]._iSteps);
    Serial.println("...");
    
    set_animation_step(loops[g_what_anim]._steps[anim_step]._iTime, loops[g_what_anim]._steps[anim_step]._iPos1, loops[g_what_anim]._steps[anim_step]._iPos2, 0);
  }
  else
  {
    if(val>0)
    {
      static int cur_pos = 0;

      if(val=='+' && cur_pos<10) cur_pos+=1;
      else if(val=='-'  && cur_pos>0) cur_pos-=1;
      else if(val=='0') cur_pos=0;

      if(cur_pos==0)
      {
        Serial.println("Position RIGHT");
        if(val=='*')
          set_animation_step(1000, _POSITION_CURRENT, _POSITION_RIGHT, 0);
      }
      else if(cur_pos==1)
      {
        Serial.println("Position HELLO1");
        if(val=='*')
          set_animation_step(1000, _POSITION_CURRENT, _POSITION_HELLO1, 0);
      }
      else if(cur_pos==2)
      {
        Serial.println("Position HELLO2");
        if(val=='*')
          set_animation_step(1000, _POSITION_CURRENT, _POSITION_HELLO2, 0);
      }
      else if(cur_pos==3)
      {
        Serial.println("Position STAND");
        if(val=='*')
          set_animation_step(1000, _POSITION_CURRENT, _POSITION_STAND, 0);
      }
      else if(cur_pos==4)
      {
        Serial.println("Position WAITFOR");
        if(val=='*')
          set_animation_step(1000, _POSITION_CURRENT, _POSITION_WAITFOR, 0);
      }
      else if(cur_pos==5)
      {
        Serial.println("Position PUMP0");
        if(val=='*')
          set_animation_step(1000, _POSITION_CURRENT, _POSITION_PUMP0, 0);
      }
      else if(cur_pos==6)
      {
        Serial.println("Position PUMP1");
        if(val=='*')
          set_animation_step(1000, _POSITION_CURRENT, _POSITION_PUMP1, 0);
      }
      else if(cur_pos==7)
      {
        Serial.println("Position SEMIPUMP0");
        if(val=='*')
          set_animation_step(1000, _POSITION_CURRENT, _POSITION_SEMIPUMP0, 0);
      }
      else if(cur_pos==8)
      {
        Serial.println("Position SEMIPUMP1");
        if(val=='*')
          set_animation_step(1000, _POSITION_CURRENT, _POSITION_SEMIPUMP1, 0);
      }
      else if(cur_pos==9)
      {
        Serial.println("Position HULK");
        if(val=='*')
          set_animation_step(1000, _POSITION_CURRENT, _POSITION_HULK, 0);
      }
      else if(cur_pos==10)
      {
        Serial.println("Position PLANE");
        if(val=='*')
          set_animation_step(1000, _POSITION_CURRENT, _POSITION_PLANE, 0);
      }
    }
  }
}

